#ifndef PUBLISH_H
#define PUBLISH_H

#include <Eigen/Dense>

#include <map>
#include <set>
#include <memory>
#include <string>
#include <fstream>
#include <mutex>

#include "sensor/point_cloud.h"
#include "transform/transform.h"
#include "publisher/visualization_msgs.h"

namespace cartographer {

class Publisher {
public:
    Publisher(const int& trajectory_id = 0);
    Publisher(const Publisher&) = delete;            // 禁用拷贝构造
    Publisher& operator=(const Publisher&) = delete; // 禁用拷贝赋值
    // 保留移动语义（如需）
    Publisher& operator=(Publisher&&) = default;
    Publisher(Publisher&&) = default;  // 启用移动语义

    void PublishOriginPointCloud(const std::vector<float>& x_coords, const std::vector<float>& y_coords);

    void PublishFilterPointCloud(const sensor::PointCloud& accumulated_semantics_in_world, const transform::Rigid3d& pose_estimated_3d);

    void PublishTrackingPointCloud(const sensor::PointCloud& accumulated_semantics_in_world);

    void PublishTrackingPose(const std::unique_ptr<transform::Rigid2d>& world_pose);

    void PublishGtPose(const Eigen::Vector3d& gt_world_pose);  

    void PublishOdometryPose(const Eigen::Vector3d& world_pose);  

    void PublishPredictPose2D(const transform::Rigid2d& world_pose);  
    void PublishPredictPose3D(const transform::Rigid3d& world_pose); 

    void PublishCurrentPointCloud(const sensor::PointCloud& current_point);

    void PublishLandmarkPosesList(const std::map<std::string, transform::Rigid3d>& landmark_poses);

    /**
     * Get all landmarks and local landmarks
     * @param all_landmarks
     * @param local_landmarks
     * @return number of landmarks in map
     */
    std::vector<Eigen::Vector3f> get_landmarks();
    std::vector<Eigen::Vector3d> get_poses();
    std::vector<Eigen::Vector3d> get_gt_poses();
    std::vector<Eigen::Vector3d> get_odometry_poses();
    std::vector<Eigen::Vector3f> get_current_landmarks();
    std::vector<Eigen::Vector3d> get_predict_poses2d();
    std::vector<Eigen::Vector3d> get_predict_poses3d();
    std::vector<Eigen::Vector3f> get_filter_landmarks();
    
    std::vector<Eigen::Vector3d> get_submap_landmarks();

    std::vector<Eigen::Vector3d> get_submap_world_pose();

    std::vector<Eigen::Vector3d> get_all_poses();

    int trajectory_id() {
        return trajectory_id_;
    }

private:
	std::vector<Eigen::Vector3f> landmarks_; 
	std::vector<Eigen::Vector3d> poses_;
    std::vector<Eigen::Vector3d> gt_poses_;	
    std::vector<Eigen::Vector3d> odometry_poses_; 
    std::vector<Eigen::Vector3f> current_landmarks_; 
    std::vector<Eigen::Vector3d> predict_poses2d_;
    std::vector<Eigen::Vector3d> predict_poses3d_;
    std::vector<Eigen::Vector3f> filter_landmarks_; 
    std::vector<Eigen::Vector3d> all_poses_;

    std::map<std::string, transform::Rigid3d> landmark_poses_;

    int trajectory_id_;

    std::mutex odometry_mutex_;
    std::mutex predict_pose2d_mutex_;
    std::mutex predict_pose3d_mutex_;
    std::mutex filter_point_cloud_mutex_;
};

} // namespace cartographer

#endif
